from MiniBotControlLib import MiniBotControlLib
from MiniBotAlgorithmLib import MiniBotAlgorithmLib
import time

######################################################################
######################################################################
####################### 操作臂构型机器人 ###########################
######################################################################
######################################################################

#声明动态库辅助对象
mclib = MiniBotControlLib()
malib = MiniBotAlgorithmLib()

print("Welcome to ManipulatorRobot Sample....")
print("Enter any key to scan robots ....")
input("")


#数字输入辅助方法
def inputNum(type=int,content=''):
  data = input(content)
  try:
    inputData = type(data)
    return inputData
  except:
   #pass
   return -1



#扫描在线机器人Start
robotsIps = []
robotsCount = 0

#当前连接的机器人IP与端口Port
robotIp = ""
robotPort = 0

#当前机器人的ID序列
_moduleIds = []



######################################################################
####################### 机器人连接及模块扫描 ###########################
######################################################################
while(1):
    cmd = 0
    if(robotsCount > 0):
        print("Enter the robot index (ex: 1)...[Input 0 to scan again]：")
        cmd = inputNum()
        
    if(cmd == 0):
         print("Scaning the robots......")

         #扫描在线机器人
         robotsIps = mclib.ScanRobots()
         robotsCount = len(robotsIps)
         print("Found robots：" + str(robotsCount))
         i = 0
         while i < robotsCount:
           print(str(i + 1) + ":" + str(robotsIps[i]))
           i = i + 1

    elif(cmd > 0 and cmd <= robotsCount):
        robotAddr = robotsIps[cmd - 1]
        ipPort = robotAddr[:robotAddr.index(",")]     #IP与端口
        ip = ipPort
        port = 8899
        if(ipPort.find(":") > 0):
           ip = ipPort[:ipPort.index(":")]
           port = int(ipPort[ipPort.index(":") + 1:])

        #连接机器人设备
        res = mclib.Connect(ip,port)
        #res = mclib.ConnectWithTimeOut(ip,port,200)
        if(res > 0):
            print("Connected")
            robotIp = ip
            robotPort = port
            break
        else:
            print("Connect Fail")


######################################################################
################# 机器人模块ID扫描 ##################################
######################################################################
while(1):
    ids = mclib.ScanId(20,50)
    idsCount = len(ids[0])
    print("Modules Total Count： " + str(idsCount))
    print("Followed：")

    c = 0
    while c < idsCount:
         print("Id： " + str(str(ids[0][c])) + "  Position:" + str(ids[1][c]))
         c = c + 1

    #默认提示扫描机器人模块ID列表
    cmd = 0
    print("Any Key to continue....... (Input 0 to scan again)")
    cmd = inputNum()
    if(cmd != 0):
        break


######################################################################
################# 机器人模块ID配置 ##################################
######################################################################
#输入操作臂ID次序
print("The id sequence above may be incorrect")
print("Please enter the manipulator ids in sequence from down to top （I T T I T G）")
_moduleIds = []

indexs = [x for x in range(6)]
for i in indexs:
   print("Please enter the " + str(i + 1) + " module's Id")
   _moduleIds.append(inputNum())

print("Ids  completed")


######################################################################
################# 机器人操作控制开始 ##################################
######################################################################
while(1):
    print("Commands：\n 1.Set Modules Angles \n 2.Set Space Coordinates \n 3.Get Modules Agnles \n")
    cmd = inputNum()
    if(cmd == 1):
        #设置模块关节角度
        joints = []
        for i in indexs:
            print("Please enter the \n" + str(i + 1) + " module's angle")
            angle = inputNum(float)
            #设置为位置模式
            mclib.SetMotorMode(_moduleIds[i],0)
            #等待50MS，太快可能会出现设置失败的情况
            time.sleep(0.05)
            #设置位置
            mclib.SetPosition(_moduleIds[i],angle)
            joints.append(angle)

        print("Now The Space Coordinates is:")
        sc = malib.CalJointAngleToTerminalCartesian(joints)

        print("X：" + str(sc[0]) + "\nY:" + str(sc[1]) + "\nZ:" + str(sc[2]) + "\nRoll:" + str(sc[3]) + "\nPitch:" + str(sc[4]) + "\nYaw:" + str(sc[5]))

    elif cmd == 2:
        ps = []
        print("Please enter the Space Coordinates:")
        ps.append(inputNum(float,"Enter X:"))
        ps.append(inputNum(float,"Enter Y:"))
        ps.append(inputNum(float,"Enter Z:"))
        ps.append(inputNum(float,"Enter Roll:"))
        ps.append(inputNum(float,"Enter Pitch:"))
        ps.append(inputNum(float,"Enter Yaw:"))

        print("Now The I T T I T Angles is:")
        joints = malib.CalTerminalCartesianToJointAngle(ps)
        print(str(joints))
        i = 0
        while i < len(joints): 
          #设置位置模式
          mclib.SetMotorMode(_moduleIds[i],0)
          #等待50MS，太快可能会出现设置失败的情况
          time.sleep(0.05)
          #设置位置
          mclib.SetPosition(_moduleIds[i],joints[i])
          i = i + 1

    elif(cmd == 3):
        print("Getting modules's angle ")
        for i in indexs:
          angle = mclib.GetPosition(_moduleIds[i])
          print("Module " + str(_moduleIds[i]) + " ：" + str(angle))

print("Enter any key to continue ....")
input()
    


